#include <math.h>
#include <string.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <libxml/encoding.h>
#include <libxml/xmlwriter.h>
#include <libxml/parser.h>
#include "gpiod.h"
#include "microhttpd.h"
#ifdef MHD_HAVE_LIBMAGIC
#include <magic.h>
#endif /* MHD_HAVE_LIBMAGIC */
#define SOFTWARE_NAME "webrobot"
#define PI 3.14
#define DEFAULT_LINEAR 1
#define DEFAULT_ANGULAR 1

struct ts_robot {
  int port;
  int64_t controlTime;
  void *leftMotor;
  void *rightMotor;
};
struct ts_motor {
  char *name;
  void *negativeGpio;
  void *positiveGpio;
};
struct ts_gpio {
  char *name;
  int chipNum;
  int lineOffset;
  void *chip;
  void *line;
};
int64_t t_get_real_time(void)//us 
{
  struct timeval tv;
  if (gettimeofday(&tv, NULL) == 0) {
    return (int64_t)tv.tv_sec * 1000000 + tv.tv_usec;
  }
  return -1;
}
void robot_run(void *obj, float lx, float ly, float lz,
    float ax, float ay, float az)
{
  int ret = false;
  struct ts_robot *robot = obj;
  struct ts_motor *lm = robot->leftMotor, *rm = robot->rightMotor;
  struct ts_gpio *lpg = lm->positiveGpio, *lng = lm->negativeGpio,
                 *rpg = rm->positiveGpio, *rng = rm->negativeGpio;
  struct gpiod_line *lpl = lpg->line, *lnl = lng->line,
                    *rpl = rpg->line, *rnl = rng->line;
  if (lx > 0) {
    ret = gpiod_line_set_value(lpl, true);
    ret = gpiod_line_set_value(lnl, false);
    ret = gpiod_line_set_value(rpl, true);
    ret = gpiod_line_set_value(rnl, false);
  } else if (lx < 0) {
    ret = gpiod_line_set_value(lpl, false);
    ret = gpiod_line_set_value(lnl, true);
    ret = gpiod_line_set_value(rpl, false);
    ret = gpiod_line_set_value(rnl, true);
  } else {
    //= 0
    if (az < 0) {
      ret = gpiod_line_set_value(lpl, true);
      ret = gpiod_line_set_value(lnl, false);
      ret = gpiod_line_set_value(rpl, false);
      ret = gpiod_line_set_value(rnl, false);
    } else if (az > 0) {
      ret = gpiod_line_set_value(lpl, false);
      ret = gpiod_line_set_value(lnl, false);
      ret = gpiod_line_set_value(rpl, true);
      ret = gpiod_line_set_value(rnl, false);
    } else {
      ret = gpiod_line_set_value(lpl, false);
      ret = gpiod_line_set_value(lnl, false);
      ret = gpiod_line_set_value(rpl, false);
      ret = gpiod_line_set_value(rnl, false);
    }
  }
  if (ret < 0) {
    fprintf(stderr, "L%d", __LINE__);
    exit(1);
  }
}
char *callback(const char *method, const char *url, const char *data,
    void *user_data)
{
  struct ts_robot *robot = user_data;
  char *res = NULL, *cmd;
  float percent = 0;
  xmlDocPtr doc;
  xmlNodePtr root;
  xmlChar *str = NULL;

  printf("[++++]L%d method:%s url:%s data:%s//end\n",
      __LINE__, method, url, data);
  if (!robot) {
    printf("[ERROR] NO ROBOT\n");
    exit(1);
  }
  if (!strcmp(url, "/robot")) {
    robot->controlTime = t_get_real_time();
    doc = xmlParseDoc(BAD_CAST data);
    if (!doc)
      return res;
    //printf("[++++]L%d\n", __LINE__);
    root = xmlDocGetRootElement(doc);
    str = xmlGetProp(root, BAD_CAST "percent");
    if (str) {
      percent = atof((char *)str);
      xmlFree(str);
    }
    str = xmlGetProp(root, BAD_CAST "cmd");
    cmd = (char *)str;
    //printf("[++++]L%d cmd:%s\n", __LINE__, cmd);
    if (!strcmp(cmd, "up")) {
      robot_run(robot, percent, 0.0, 0.0, 0.0, 0.0, 0.0);
    } else if (!strcmp(cmd, "down")) {
      robot_run(robot, -percent, 0.0, 0.0, 0.0, 0.0, 0.0);
    } else if (!strcmp(cmd, "left")) {
      robot_run(robot, 0.0, 0.0, 0.0, 0.0, 0.0, percent);
    } else if (!strcmp(cmd, "right")) {
      robot_run(robot, 0.0, 0.0, 0.0, 0.0, 0.0, -percent);
    } else if (!strcmp(cmd, "stop")) {
      robot_run(robot, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    } else {
      robot_run(robot, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    }
    if (str)
      xmlFree(str);
    xmlFreeDoc(doc);
    char *sucess = "<test>success</test>", *now = NULL;
    now = sucess;
    res = calloc(1, strlen(now)+1);
    strcpy(res, now);
  }
  return res;
}
static enum MHD_Result
send_msg (struct MHD_Connection *connection, const char *page)
{
  enum MHD_Result ret;
  struct MHD_Response *response;
  //response = MHD_create_response_from_buffer_static (
  response = MHD_create_response_from_buffer (
      strlen (page), (void *)page, MHD_RESPMEM_PERSISTENT);
  if (! response)
    return MHD_NO;
  ret = MHD_queue_response (connection, MHD_HTTP_OK, response);
  MHD_destroy_response (response);
  return ret;
}
static struct MHD_Response *file_not_found_response;
char *html_path = "/usr/local/share/ros2httpd/html";
static enum MHD_Result answer_to_connection (
    void *cls, struct MHD_Connection *connection,
    const char *url, const char *method,
    const char *version, const char *upload_data,
    size_t *upload_data_size, void **req_cls)
{
  char *root = cls;
  struct MHD_Response *response;
  int fd;//, buf_size = 2048;
  struct stat buf;
  enum MHD_Result ret;
  //enum MHD_Result ret;
  //printf("%s\n", __func__);
  //printf("method:%s url:%s\n", method, url);
  if (NULL == *req_cls) {
    *req_cls = (void *)1;
    return MHD_YES;
  }
  if (0 == strcmp (method, "GET")) {
//    return send_msg (connection, "\nget\n");
#ifdef MHD_HAVE_LIBMAGIC
    char file_data[MAGIC_HEADER_SIZE];
    ssize_t got;
#endif /* MHD_HAVE_LIBMAGIC */
    const char *mime;
    char filename[1024], *index_path = "/index.html";
    const char *url_path;
    if ( (0 != strcmp (method, MHD_HTTP_METHOD_GET)) &&
         (0 != strcmp (method, MHD_HTTP_METHOD_HEAD)) )
      return MHD_NO;    /* unexpected method (we're not polite...) */
    fd = -1;
    if (!strcmp(url, "/")) {
      url_path = index_path;
    } else {
      url_path = url;
    }
    if (root) {
      snprintf(filename, sizeof(filename), "%s%s", root, url_path);
      printf(" ROOT PATH:%s\n", root);
    } else {
      printf("[ERROR] NO ROOT PATH\n");
      exit(1);
    }
    fd = open(filename, O_RDONLY);
    //fd = open (&url[1], O_RDONLY);
    if ( (-1 != fd) && ( (0 != fstat (fd, &buf)) ||
           (! S_ISREG (buf.st_mode)) ) ) {
      (void) close (fd);
      fd = -1;
    }
    if (-1 == fd) {
      printf("[ERROR] NO SUCH FILE:%s\n", filename);
      return MHD_queue_response (connection, MHD_HTTP_NOT_FOUND,
                                 file_not_found_response);
    }
#ifdef MHD_HAVE_LIBMAGIC
    got = read (fd, file_data, sizeof (file_data));
    (void) lseek (fd, 0, SEEK_SET);
    if (-1 != got)
      mime = magic_buffer (magic, file_data, got);
    else
#endif /* MHD_HAVE_LIBMAGIC */
    mime = NULL;
    if (NULL == (response = MHD_create_response_from_fd (
            buf.st_size, fd))) {
      (void) close (fd);
      return MHD_NO;
    }
    if (NULL != mime)
      (void) MHD_add_response_header (response,
          MHD_HTTP_HEADER_CONTENT_TYPE, mime);
    ret = MHD_queue_response (connection, MHD_HTTP_OK, response);
    if (ret == MHD_NO)
      return ret;
    MHD_destroy_response (response);
    return ret;
  }
  if (0 == strcmp (method, "POST")) {
    if (*upload_data_size != 0) {
      *req_cls = strndup(upload_data, *upload_data_size);
      *upload_data_size = 0;
      return MHD_YES;
    }
    if (*req_cls != (void *)1) {
      char *data = (char *)*req_cls;
      callback(method, url, data, cls);
      free(data);
    }
    return send_msg (connection, "\npost\n");
  }
  return send_msg (connection, "\ninvalid method\n");
}
int main (int argc, char **argv)
{
#if defined(__x86_64__) || defined(_M_X64)
  printf("This is an x86_64 (or AMD64) architecture.\n");
#elif defined(__aarch64__)
  printf("This is an aarch64 (ARM64) architecture.\n");
#else
  printf("Unknown architecture.\n");
#endif

  xmlDocPtr doc;
  xmlNodePtr root_node, robot_node, motor_node, gpio_node;
  xmlChar *xml_char = NULL;
  char *filename;
  if ((argc == 3) && !strcmp(argv[1], "-c")) {
    filename = argv[2];
  } else {
    printf("Usage: %s [OPTION]... [FILE]...\n\
%s\n\
-c\tconfig file\n\
Examples:\n\
\t%s -c config.xml\n", SOFTWARE_NAME, SOFTWARE_NAME, SOFTWARE_NAME);
    exit(1);
  }
  if (access(filename, F_OK) == 0) {
    doc = xmlParseFile(filename);
    printf("[DEBUG]xml_driver__file:%s\n", filename);
  } else {
    printf("[----ERROR] no %s\n", filename);
    exit(1);
   // return NULL;
  }
  root_node = xmlDocGetRootElement(doc);
  robot_node = xmlFirstElementChild(root_node);
  struct ts_robot *robot = calloc(1, sizeof(*robot));
  xml_char = xmlGetProp(robot_node, BAD_CAST "port");
  robot->port = atoi((char *)xml_char);
  xmlFree(xml_char);
  motor_node = xmlFirstElementChild(robot_node);
  while (motor_node) {
    struct ts_motor *motor = calloc(1, sizeof(*motor));
    xml_char = xmlGetProp(motor_node, BAD_CAST "name");
    if (strcmp((char *)xml_char, "left") == 0) {
      robot->leftMotor = motor;
    } else {
      robot->rightMotor = motor;
    }
    xmlFree(xml_char);
    gpio_node = xmlFirstElementChild(motor_node);
    while (gpio_node) {
      struct ts_gpio *gpio = calloc(1, sizeof(*gpio));
      int num, offset;
      xml_char = xmlGetProp(gpio_node, BAD_CAST "name");
      if (strcmp((char *)xml_char, "negative") == 0) {
        motor->negativeGpio = gpio;
      } else {
        motor->positiveGpio = gpio;
      }
      xmlFree(xml_char);
      xml_char = xmlGetProp(gpio_node, BAD_CAST "chipNum");
      num = atoi((char *)xml_char);
      xmlFree(xml_char);
      xml_char = xmlGetProp(gpio_node, BAD_CAST "lineOffset");
      offset = atoi((char *)xml_char);
      xmlFree(xml_char);
      printf("num:offset %d:%d\n", num, offset);
      struct gpiod_chip *chip = NULL;
      struct gpiod_line *line = NULL;
      chip = gpiod_chip_open_by_number(num);
      if (!chip) {
        fprintf(stderr, "L%d", __LINE__);
        return -1;
      }
      line = gpiod_chip_get_line(chip, offset);
      if (!line) {
        fprintf(stderr, "request output");
        return -1;
      }
      int ret;
      ret = gpiod_line_request_output(line, "example", 0);
  //        GPIOD_LINE_ACTIVE_STATE_LOW);
      if (ret != 0) {
        printf("%s %d %s:%d\n",
            __FILE__, __LINE__, __func__, ret);
        return false;
      }
      gpio->line = line;
      gpio_node = xmlNextElementSibling(gpio_node);
    }
    motor_node = xmlNextElementSibling(motor_node);
  }
  //httpd
  int port = robot->port;
  struct MHD_Daemon *daemon;

  daemon = MHD_start_daemon (
      MHD_USE_AUTO | MHD_USE_INTERNAL_POLLING_THREAD,
      port, NULL, NULL,
      &answer_to_connection, robot,
      NULL, MHD_OPTION_END);
  if (NULL == daemon)
    return 1;
  while (1) {
    if (robot->controlTime) {
      if ((t_get_real_time() - robot->controlTime) > 1000*100) {
        robot_run(robot, 0, 0, 0, 0, 0, 0);
        robot->controlTime = 0;
      }
    }
    usleep(10000);
  }
  MHD_stop_daemon (daemon);
  return 0;
}

